﻿/* ------------------------------------------------------------------------- */
/*
 * C O M P A N Y   P R O P R I E T A R Y
 * use or disclosure of data contained in this file is subject to the
 * requirements an the end of this file
 *
 * CLASSIFICATION: OFFEN / UNCLASSIFIED
 */
/* ------------------------------------------------------------------------- */

/* ######################################################################### */
/** \file M44_UpdateReference.cs
 * \brief	This file contains the ObjectFinder function that 
 *  
 * This file contains the CObjectFinder function that 
 * 
 *
 * PROJEKT:   NETWORKED MULTI-ROBOT SYSTEMS
 *
 * Copyright (c) 2008 SENER - Universidad Politécnica de Madrid.
 *
 * \author    Antonio Matta \n
 *            Universidad Politécnica de Madrid \n
 *            Grupo de Robótica y Cibernética \n
 * \version   0.01
 * \date      2009-11-20
 *
 * \n \n
 * Versionshistory: \n
 * -----------------
 * - Version 0.01:   Antonio MATTA                          2009-11-20 \n
 *      First implementation of the M44_UpdateReference class.
 *      
/* ######################################################################### */
using System;
using System.Collections.Generic;
using System.Text;

/*Using Interfaces classes*/
using Interfaces;
using Interfaces.Complex;
using Interfaces.Basis;

/*Using the 3D rendering engine modules*/
using xna = Microsoft.Xna.Framework;

/*This is for using lists*/
using System.Collections;

/*Using the C Sharp Matrix Library*/
using NMRS.CSML;


/* ------------------------------------------------------------------------- */
/**	\namespace NMRS.M44_SensorFusionAndMapping
 *
 *  \brief	This is the namespace of the M44 Sensor Fusion and Mapping module
 */
/* ------------------------------------------------------------------------- */
namespace NMRS.M44_SensorFusionAndMapping
{
    /* ------------------------------------------------------------------------- */
    /**	\class M44_UpdateReference
     * 
     *  \brief  This is the class of the M44_ObjectFinder function.
     *
     */
    /* ------------------------------------------------------------------------- */
    public static class M44_UpdateReference
    {
        /* ------------------------------------------------------------------------- */
        /**	\fn public static BackgroundReference UpdateReference(float[,] D_ref, float[,] V_ref, cl_3DLrfMeasure Laser, float[,] D_0, float[,] D_00, int[,] D_Obj )
         * 
         *  \brief 
         *  
         *  \param[in] float[,] D_ref                       Reference distance matrix
         *  \param[in] float[,] V_ref                       variance matrix 
         *  \param[in] float[,] cl_3DLrfMeasure Laser       actual laser measurement
         *  \param[in] float[,] D_0                         last laser distance matrix
         *  \param[in] float[,] D_00                        last last laser distance matrix
         *  \param[in] int[,] D_Obj                         object matrix
         *  
         *  
         *  \param[out] BackgroundReference str_Reference     Updated Reference structure
         *
         */
        /* ------------------------------------------------------------------------- */
        public static BackgroundReference UpdateReference(float[,] D_ref, 
                                                            float[,] V_ref, 
                                                            cl_3DLrfMeasure Laser, 
                                                            float[,] D_0, 
                                                            float[,] D_00, 
                                                            int[,] D_Obj )
        {
            /*Variables*/
            BackgroundReference str_Reference = new BackgroundReference(Laser.s_i, Laser.s_n);
            float mean_D;
            float var_D;

            for (int i = 0; i < Laser.s_i; i++)
            {
                for (int j = 0; j < Laser.s_n; j++)
                {

                    if (D_Obj[i, j] == 0)
                    {
                        mean_D = (Laser.tf_3DLrfDistance[i, j] + D_0[i, j] + D_00[i, j]) / 3;
                        var_D =  (float)(Math.Pow( (Laser.tf_3DLrfDistance[i, j] - mean_D), 2.0) +  
                                 Math.Pow( (D_0[i, j] - mean_D), 2.0) + 
                                 Math.Pow( (D_00[i, j] - mean_D),2.0));
                        if (var_D < V_ref[i, j])
                        {
                            V_ref[i, j] = var_D;
                            D_ref[i, j] = mean_D;
                        }
                        else if (var_D < 4 * V_ref[i, j] && mean_D > D_ref[i, j])
                        {
                            V_ref[i, j] = var_D;
                            D_ref[i, j] = mean_D;
                        }
                    }
                }
            }
            str_Reference.tf_Dref = V_ref;
            str_Reference.tf_Vref = D_ref;
            return str_Reference;
        }       
    }
}
